#ifdef KINECT

#include "inc\GUI\kinect\kinectsnapshotrecorderdialog.h"
#include "ui_kinectsnapshotrecorderdialog.h"

#include "Core/Robot/nullbot.h"

KinectSnapshotRecorderDialog::KinectSnapshotRecorderDialog(kinect::Kinect* kinect, QWidget *parent) :
    KinectDialog(kinect, parent),
    ui(new Ui::KinectSnapshotRecorderDialog),
    _setFrame(new KinectSnapshotSetFrame()),
    _recorderFrame(new KinectSnapshotRecorderFrame()),
    _keyframer(new kinect::TroyKeyframeGenerator(this))
{
    ui->setupUi(this);

    connect(_keyframer, SIGNAL(poseAdded(int)), this, SLOT(addSnapshot(int)));

    _snapshotSet = ui->stack->addWidget(_setFrame);
    connect(_setFrame, SIGNAL(snapshotRequest()), this, SLOT(takeSnapshot()));
    connect(_setFrame, SIGNAL(snapshotDeleteRequest(int)), this, SLOT(deleteSnapshot(int)));
    _recorder = ui->stack->addWidget(_recorderFrame);
    connect(_keyframer, SIGNAL(poseStarted(int)), _recorderFrame, SLOT(startPose(int)));
    connect(_keyframer, SIGNAL(poseEnded(int)), _recorderFrame, SLOT(endPose(int)));
    connect(_keyframer, SIGNAL(poseCompared(int,float)), _recorderFrame, SLOT(setPoseMatch(int,float)));
    connect(_recorderFrame, SIGNAL(startRequested()), this, SLOT(start()));
    connect(_recorderFrame, SIGNAL(stopRequested()), this, SLOT(stop()));

    connect(ui->prevButton, SIGNAL(clicked()), this, SLOT(prev()));
    connect(ui->nextButton, SIGNAL(clicked()), this, SLOT(next()));

    connect(ui->cancelButton, SIGNAL(clicked()), this, SLOT(reject()));
    connect(ui->finishButton, SIGNAL(clicked()), this, SLOT(accept()));

    connect(ui->stack, SIGNAL(currentChanged(int)), this, SLOT(updateFrame(int)));
    ui->stack->setCurrentIndex(_snapshotSet);

    ui->troyPreview->mirror(true);

    maximizeSize();
}

KinectSnapshotRecorderDialog::~KinectSnapshotRecorderDialog()
{
    delete ui;
}

#ifdef WIIMOTE
void KinectSnapshotRecorderDialog::associateWiimote(WiimoteStatusDialog *dialog) {
    updateWiimoteCommands();
}

void KinectSnapshotRecorderDialog::updateWiimoteCommands() {
    if (hasWiimoteDialog()) {
        getWiimoteDialog()->clearCommands();
        getWiimoteDialog()->setCommand(qWiimote::B, "Cancel", this, SLOT(reject()));
        getWiimoteDialog()->setCommand(qWiimote::Home, "Reset", getKinectSkeleton(), SLOT(reset()));
        if (ui->stack->currentIndex() == _snapshotSet) {
            getWiimoteDialog()->setCommand(qWiimote::A, "Take Snapshot", this, SLOT(takeSnapshot()));
            getWiimoteDialog()->setCommand(qWiimote::Plus, "Next", this, SLOT(next()));
            getWiimoteDialog()->setCommand(qWiimote::Minus, "Cancel", this, SLOT(reject()));
        } else if (ui->stack->currentIndex() == _recorder) {
            getWiimoteDialog()->setCommand(qWiimote::A, "Start/Stop Recording", this, SLOT(toggle()));
            getWiimoteDialog()->setCommand(qWiimote::Minus, "Previous", this, SLOT(prev()));
            if (ui->finishButton->isEnabled()) {
                getWiimoteDialog()->setCommand(qWiimote::Plus, "Finish", this, SLOT(accept()));
            }
            getWiimoteDialog()->linkPositiveFeedback(_keyframer, SIGNAL(poseStarted()));
        }
    }
}
#endif

RobotAction* KinectSnapshotRecorderDialog::getRecordedAction() {
    RobotAction* action = ui->troyPreview->takeAction();
    if (action != NULL) {
        action->stop();
        action->setParent(NULL);
    }
    return action;
}

void KinectSnapshotRecorderDialog::kinectKeyFrame(MotionKeyFrame keyFrame) {
    if (ui->stack->currentIndex() == _snapshotSet) {
        _setFrame->setPreview(keyFrame);
    } else if (ui->stack->currentIndex() == _recorder) {
        _keyframer->processFrame(getTroyState());
    }
}

void KinectSnapshotRecorderDialog::start() {
    if (ui->stack->currentIndex() != _recorder) {
        ui->stack->setCurrentIndex(_recorder);
    }
    _keyframer->start();
    _recorderFrame->started();
}

void KinectSnapshotRecorderDialog::stop() {
    _keyframer->stop();
    _recorderFrame->stopped();

    // Build the action that was recorded
    RobotAction* action = ui->troyPreview->takeAction();
    if (action != NULL) {
        action->deleteLater();
    }
    action = new RobotAction(new NullBot(), "New Recorded Action");
    MotionKeyFrame frame;
    QMap<int, int> frames = _keyframer->getFrames();
    foreach (int frameTime, frames.keys()) {
        frame = troyStateToKeyFrame(_keyframer->getPose(frames.find(frameTime).value()));
        frame.timestamp = frameTime;
        if (frame.timestamp >= action->motionDuration()) {
            action->setMotionDuration(frame.timestamp + 10);
        }
        action->addKeyFrame(frame);
    }
    ui->troyPreview->setAction(action);
    ui->finishButton->setDisabled(false);
    ui->finishButton->show();
#ifdef WIIMOTE
    updateWiimoteCommands();
#endif
}

void KinectSnapshotRecorderDialog::toggle() {
    if (_keyframer->active()) {
        stop();
    } else {
        start();
    }
}

void KinectSnapshotRecorderDialog::takeSnapshot() {
    if (!_keyframer->active()) {
        _keyframer->addPose(getTroyState());
        getTroyState()->hasLeftArm(); //validate the cache for testing
    }
}

void KinectSnapshotRecorderDialog::addSnapshot(int id) {
    if (!_keyframer->active()) {
        _setFrame->addSnapshot(id, troyStateToKeyFrame(_keyframer->getPose(id)));
        _recorderFrame->addPose(id, troyStateToKeyFrame(_keyframer->getPose(id)));
    }
}

void KinectSnapshotRecorderDialog::deleteSnapshot(int id) {
    if (!_keyframer->active()) {
        _keyframer->deletePose(id);
        _setFrame->deleteSnapshot(id);
        _recorderFrame->deletePose(id);
    }
}

void KinectSnapshotRecorderDialog::updateFrame(int stackId) {
    if (stackId == _snapshotSet) {
        ui->nextButton->setDisabled(false);
        ui->nextButton->show();
        ui->prevButton->setDisabled(true);
        ui->prevButton->hide();
        //ui->cancelButton->setDisabled(true);
        //ui->cancelButton->hide();
        ui->finishButton->setDisabled(true);
        ui->finishButton->hide();
        ui->troyPreview->hide();
    } else if (stackId == _recorder) {
        ui->nextButton->setDisabled(true);
        ui->nextButton->hide();
        ui->prevButton->setDisabled(false);
        ui->prevButton->show();
        ui->troyPreview->show();
    } else {
        ui->stack->setCurrentIndex(_snapshotSet);
    }
#ifdef WIIMOTE
    updateWiimoteCommands();
#endif
}

void KinectSnapshotRecorderDialog::next() {
    if (ui->stack->currentIndex() == _snapshotSet) {
        ui->stack->setCurrentIndex(_recorder);
    }
}

void KinectSnapshotRecorderDialog::prev() {
    if (ui->stack->currentIndex() == _recorder) {
        ui->stack->setCurrentIndex(_snapshotSet);
    }
}

#endif
